(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Iterator container

Description: This tutorial teaches you how to use the Iterator container.

Tutorial Level: BEGINNER

Next Tutorial: Wrapping a SMACH container with actionlib

Overview

The iterator allows you to loop through a state or states until success conditions are met. This tutorial demonstrates how to use an iterator to sort a list of numbers into evens and odds.

The Code

Below is the full state machine, create a file called iterator_tutorial.py and copy the following code:

https://raw.githubusercontent.com/eacousineau/executive_smach_tutorials/hydro-devel/smach_tutorials/examples/iterator_tutorial.py

   2 import roslib; roslib.load_manifest('smach')
   3 roslib.load_manifest('smach_ros')
   4 import rospy
   5 
   6 import smach
   7 from smach import Iterator, StateMachine, CBState
   8 from smach_ros import ConditionState, IntrospectionServer
   9 
  10 def construct_sm():
  11     sm = StateMachine(outcomes = ['succeeded','aborted','preempted'])
  12     sm.userdata.nums = range(25, 88, 3)
  13     sm.userdata.even_nums = []
  14     sm.userdata.odd_nums = []
  15     with sm:
  16         tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
  17                                input_keys = ['nums', 'even_nums', 'odd_nums'],
  18                                it = lambda: range(0, len(sm.userdata.nums)),
  19                                output_keys = ['even_nums', 'odd_nums'],
  20                                it_label = 'index',
  21                                exhausted_outcome = 'succeeded')
  22         with tutorial_it:
  23             container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
  24                                         input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
  25                                         output_keys = ['even_nums', 'odd_nums'])
  26             with container_sm:
  27                 #test wether even or odd
  28                 StateMachine.add('EVEN_OR_ODD',
  29                                  ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, 
  30                                                 input_keys=['nums', 'index']),
  31                                  {'true':'ODD',
  32                                   'false':'EVEN' })
  33                 #add even state
  34                 @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
  35                                     output_keys=['odd_nums'], 
  36                                     outcomes=['succeeded'])
  37                 def even_cb(ud):
  38                     ud.even_nums.append(ud.nums[ud.index])
  39                     return 'succeeded'
  40                 StateMachine.add('EVEN', CBState(even_cb), 
  41                                  {'succeeded':'continue'})
  42                 #add odd state
  43                 @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], 
  44                                     output_keys=['odd_nums'], 
  45                                     outcomes=['succeeded'])
  46                 def odd_cb(ud):
  47                     ud.odd_nums.append(ud.nums[ud.index])
  48                     return 'succeeded'
  49                 StateMachine.add('ODD', CBState(odd_cb), 
  50                                  {'succeeded':'continue'})
  51             #close container_sm
  52             Iterator.set_contained_state('CONTAINER_STATE', 
  53                                          container_sm, 
  54                                          loop_outcomes=['continue'])
  55         #close the tutorial_it
  56         StateMachine.add('TUTORIAL_IT',tutorial_it,
  57                      {'succeeded':'succeeded',
  58                       'aborted':'aborted'})
  59     return sm
  60 
  61 def main():
  62     rospy.init_node("iterator_tutorial")
  63     sm_iterator = construct_sm()
  64 
  65     # Run state machine introspection server for smach viewer
  66     intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
  67     intro_server.start()
  68 
  69 
  70     outcome = sm_iterator.execute()
  71 
  72     rospy.spin()
  73     intro_server.stop()
  74 
  75 if __name__ == "__main__":
  76     main()

The Code Explained

Start by constructing the iterator, the constructor takes the following arguments:

__init__(self, outcomes, input_keys, output_keys, it=[], it_label='it_data', exhausted_outcome='exhausted')

In this example, the outcomes now include preempted, a default outcome of the iterator. The it argument is the list of objects that will be iterated over and the it_label is key that holds the current value of the of the item in the it list. The exhausted argument should be set to the preferred state machine outcome, in this example the iterator outcome is succeeded when the iterator is done looping through the it list.

  18         tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'],
  19                                input_keys = ['nums', 'even_nums', 'odd_nums'],
  20                                it = lambda: range(0, len(sm.userdata.nums)),
  21                                output_keys = ['even_nums', 'odd_nums'],
  22                                it_label = 'index',
  23                                exhausted_outcome = 'succeeded')

Now add a container and create the states for sorting the list into even and odd.

  24         with tutorial_it:
  25             container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
  26                                         input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
  27                                         output_keys = ['even_nums', 'odd_nums'])
  28             with container_sm:
  29                 #test wether even or odd
  30                 StateMachine.add('EVEN_OR_ODD',
  31                                  ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, 
  32                                                 input_keys=['nums', 'index']),
  33                                  {'true':'ODD',
  34                                   'false':'EVEN' })
  35                 #add even state
  36                 @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
  37                                     output_keys=['odd_nums'], 
  38                                     outcomes=['succeeded'])
  39                 def even_cb(ud):
  40                     ud.even_nums.append(ud.nums[ud.index])
  41                     return 'succeeded'
  42                 StateMachine.add('EVEN', CBState(even_cb), 
  43                                  {'succeeded':'continue'})
  44                 #add odd state
  45                 @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], 
  46                                     output_keys=['odd_nums'], 
  47                                     outcomes=['succeeded'])
  48                 def odd_cb(ud):
  49                     ud.odd_nums.append(ud.nums[ud.index])
  50                     return 'succeeded'
  51                 StateMachine.add('ODD', CBState(odd_cb), 
  52                                  {'succeeded':'continue'})

Add the container to the iterator:

  53             #close container_sm
  54             Iterator.set_contained_state('CONTAINER_STATE', 
  55                                          container_sm, 
  56                                          loop_outcomes=['continue'])

Finish by adding the iterator to the top level state machine:

  57         #close the tutorial_it
  58         StateMachine.add('TUTORIAL_IT',tutorial_it,
  59                      {'succeeded':'succeeded',
  60                       'aborted':'aborted'})

Running the Code

Make sure that a roscore is running:

$ roscore 

Run the smach_viewer so you can see the results:

$ rosrun smach_viewer smach_viewer.py

Now run the state machine:

$ sudo chmod a+x iterator_tutorial.py
$ python ./iterator_tutorial.py

smach_tutorial.png

Wiki: smach/Tutorials/Iterator container (last edited 2017-01-24 11:26:57 by GertKanter)